import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    urdf_package_path = get_package_share_directory('botarm_description')
    default_urdf_path = os.path.join(urdf_package_path,'urdf','Sevenbot_urdf.xacro')
    # default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot_model.rviz')
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model',default_value=str(default_urdf_path),description='加载的模型文件路径')

    sub_command_result = launch.substitutions.Command(['xacro ',launch.substitutions.LaunchConfiguration('model')])
    robot_description_value = launch_ros.parameter_descriptions.ParameterValue(sub_command_result,value_type=str)

    with open(default_urdf_path, 'r') as file:
        urdf_content = file.read()

    action_joint_state_publisher_node = launch_ros.actions.Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui",
        name="JSP",
        output="screen",
    )

    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}],
        additional_env = {'LANG':'en_US.UTF-8'}
    )

    action_joint_state_publisher = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        output="screen",
        parameters=[{'robot_description': urdf_content}]
    )

    action_rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        # arguments=['-d', default_rviz_config_path]
    )

    return launch.LaunchDescription([
        action_joint_state_publisher_node,
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_joint_state_publisher,
        action_rviz_node,
    ])